import numpy as np
import logging


def RMSE(predicts,reals):
    return np.sqrt(np.mean((predicts-reals)**2))

def distance(u, v):
    # return distance
    return np.linalg.norm(u - v)

def solve_position(distances, anchors):
    """
    通过最小二乘法求解 (x, y) 坐标。
    
    Args:
        distances (list): 与锚节点的距离列表 [dist_1, dist_2, dist_3, ...]
        anchors (list): 锚节点的坐标列表 [(x_1, y_1), (x_2, y_2), (x_3, y_3), ...]
    
    Returns:
        tuple: 求解得到的 (x, y) 坐标
    """
    A = []
    B = []
    x1, y1 = anchors[0]
    dist1 = distances[0]
    for i in range(1, len(anchors)):
        x2, y2 = anchors[i]
        dist2 = distances[i]
        A.append([2 * (x2 - x1), 2 * (y2 - y1)])
        B.append([dist1**2 - dist2**2 - x1**2 + x2**2 - y1**2 + y2**2])
    
    A = np.array(A)
    B = np.array(B)
    
    # 使用最小二乘法求解 Ax = B
    position = np.linalg.lstsq(A, B, rcond=None)[0]
    
    return position[0][0], position[1][0]

if __name__ == "__main__":
    # 示例数据
    distances = [5.0, 6.0, 7.0]
    anchors = [(1.0, 2.0), (3.0, 4.0), (5.0, 6.0)]

    # 求解 (x, y) 坐标
    x, y = solve_position(distances, anchors)
    print(f"求解得到的坐标: ({x}, {y})")